/*
 * MPEGGranule.cpp
 *        -- define granule format for MPEG frame
 *
 * Author: Qiu Shuang
 * Date: Mar 2nd, 2011
 */
#include <cstring>
#include <cmath>
#include "MPEGGranule.h"
#include "MPEGFrameHeader.h"
#include "MPEGSideInfo.h"
#include "MPEGFrameData.h"

#ifdef DEBUG
#include <assert.h>
#endif

/* constructor */
MPEGGranule::MPEGGranule(FILE *mpeg, MPEGFrameHeader *header, MPEGSideInfo *sideinfo, MPEGFrameData *scalefactor, int granule):
    _frame_header(header), _sideinfo(sideinfo), _scalefactor(scalefactor), _granule_no(granule){
    /* zero out all the data buffer */
    memset((void *)_dehuffman_samples, 0, CHANNEL * sizeof(ChannelDataLong));
    memset((void *)_restored_samples, 0, CHANNEL * sizeof(ChannelDataDouble));
    memset((void *)_mdct_coefficients, 0, CHANNEL * sizeof(ChannelDataDouble));
    //memset((void *)_restored_samples, 0, CHANNEL * sizeof(ChannelDataDouble));
}

/* destructor */
MPEGGranule::~MPEGGranule(){
    /* automatically allocated, no need to free manually */
}

/* restore subband samples from huffman decoded samples */
bool MPEGGranule::restoreSamples(int channel){
    int critical_band = 0;
    int cb_begin = 0, cb_width = 0;
    int subband, subsamples;
    unsigned int next_cb_boundary, boundary;

    /* compute overall scaling for this channel and granule */
    const double globalgain = pow(2.0, (0.25 *
        (_sideinfo->globalGain(_granule_no, channel) - 210.0)));

    /* compute scalefactor scaling for this channel and granule */
    const double scalefactor_scale = -0.5 *
        (1.0 + _sideinfo->scalefacScale(_granule_no, channel));

#ifdef DEBUG
    extern std::ofstream global_gain;
    extern std::ofstream scalescale;
    extern FILE *scale_fact;
    char buffer[256];

    fprintf(scale_fact, "short window scale factor: \n");

    int window;
    int scale_band;
    for(window = 0; window < 3; window++){
        for(scale_band = 0; scale_band < 13; scale_band++){
            sprintf(buffer, "%d\t", _scalefactor->_shortwin_scalefac[_granule_no][channel][window][scale_band]);
            fprintf(scale_fact, buffer);
        }

        fprintf(scale_fact, "\n");
    }

    fprintf(scale_fact, "long window scale factor: \n");
    for(scale_band = 0; scale_band < 22; scale_band++){
        sprintf(buffer, "%d\n", _scalefactor->_longwin_scalefac[_granule_no][channel][scale_band]);
        fprintf(scale_fact, buffer);
    }

    fprintf(scale_fact, "\n");

    //global_gain << "granule number " << _granule_no << std::endl;
    //global_gain << (int)_sideinfo->globalGain(_granule_no, channel) << '\t' << globalgain << std::endl;
    scalescale << _sideinfo->scalefacScale(_granule_no, channel) << '\t' << scalefactor_scale << std::endl;
#endif

    /* choose right scalefactor band per block type */
    if (_sideinfo->windowSwitching(_granule_no, channel) &&
       (_sideinfo->windowType(_granule_no, channel) == 2)){
       if (_sideinfo->mixedWindow(_granule_no, channel)){
           /* mixed block */
           next_cb_boundary = _frame_header->longWinScalefactorBandBound(1);

#ifdef DEBUG
//    global_gain << "next criterial band boundary(long): " << next_cb_boundary << std::endl;
#endif

       }else{
           /* pure short block */
           next_cb_boundary = _frame_header->shortWinScalefactorBandBound(1) * 3;
           cb_width = _frame_header->shortWinScalefactorBandBound(1);
#ifdef DEBUG
//    global_gain << "next criterial band boundary(short): " << next_cb_boundary << std::endl;
//    global_gain << "criterial band width: " << cb_width << std::endl;
#endif
           cb_begin = 0;
       }
    }else{
        /* long window: 0, 1, 3 */
        next_cb_boundary = _frame_header->longWinScalefactorBandBound(1);
#ifdef DEBUG
//    global_gain << "next criterial band boundary(long): " << next_cb_boundary << std::endl;
#endif
    }

    /* apply formula per block type */
    for(subband = 0; subband < SUBBAND_COUNTS; subband++){
        for(subsamples = 0; subsamples < SUBBAND_SAMPLES; subsamples++){
            boundary = subband * SUBBAND_SAMPLES + subsamples;
            if (boundary == next_cb_boundary){
                /* adjust critical band boundary */
                if (_sideinfo->windowSwitching(_granule_no, channel) &&
                   (_sideinfo->windowType(_granule_no, channel) == 2)){
                       if (_sideinfo->mixedWindow(_granule_no, channel)){
                       /* mixed window */
#ifdef DEBUG
    scalescale << _frame_header->longWinScalefactorBandBound(8) << std::endl;
#endif
                           if (boundary == _frame_header->longWinScalefactorBandBound(8)){
                               next_cb_boundary =
                                   _frame_header->shortWinScalefactorBandBound(4) * 3;
                               critical_band = 3;
                               cb_width =
                                   _frame_header->shortWinScalefactorBandBound(critical_band + 1) -
                                   _frame_header->shortWinScalefactorBandBound(critical_band);
                               cb_begin = _frame_header->shortWinScalefactorBandBound(critical_band) * 3;
//#ifdef DEBUG
//    global_gain << "next criterial band boundary(long): " << next_cb_boundary << std::endl;
//    global_gain << "criterial band width: " << cb_width << std::endl;
//    global_gain << "criterial band begin: " << cb_begin << std::endl;
//#endif
                           }else if(boundary < _frame_header->longWinScalefactorBandBound(8)){
                               critical_band++;
                               next_cb_boundary = _frame_header->longWinScalefactorBandBound(critical_band + 1);
#ifdef DEBUG
//    global_gain << "criterial band: " << critical_band << std::endl;
//    global_gain << "next criterial band boundary(long): " << next_cb_boundary << std::endl;
#endif
                           }else{
                               critical_band++;
                               next_cb_boundary = _frame_header->shortWinScalefactorBandBound(critical_band + 1) * 3;
                               cb_width = _frame_header->shortWinScalefactorBandBound(critical_band + 1) -
                                   _frame_header->shortWinScalefactorBandBound(critical_band);
                               cb_begin = _frame_header->shortWinScalefactorBandBound(critical_band) * 3;
#ifdef DEBUG
//    global_gain << "next criterial band boundary(short): " << next_cb_boundary << std::endl;
//    global_gain << "criterial band width: " << cb_width << std::endl;
//    global_gain << "criterial band begin: " << cb_begin << std::endl;
//    global_gain << "criterial band: " << critical_band << std::endl;
#endif
                           }
                       }else{           /* pure short blocks */
                           critical_band++;
                           next_cb_boundary = _frame_header->shortWinScalefactorBandBound(critical_band + 1) * 3;
                           cb_width = _frame_header->shortWinScalefactorBandBound(critical_band + 1) -
                               _frame_header->shortWinScalefactorBandBound(critical_band);
                           cb_begin = _frame_header->shortWinScalefactorBandBound(critical_band) * 3;
#ifdef DEBUG
//    global_gain << "next criterial band boundary(short): " << next_cb_boundary << std::endl;
//    global_gain << "criterial band width: " << cb_width << std::endl;
//    global_gain << "criterial band begin: " << cb_begin << std::endl;
//    global_gain << "criterial band: " << critical_band << std::endl;
#endif
                       }
                }else{                  /* long block */
                    critical_band++;
                    next_cb_boundary = _frame_header->longWinScalefactorBandBound(critical_band + 1);
#ifdef DEBUG
//    global_gain << "next criterial band boundary(long): " << next_cb_boundary << std::endl;
//    global_gain << "criterial band: " << critical_band << std::endl;
#endif
                }
            }

            /* use overall global scaling */
            _restored_samples[channel][subband][subsamples] = globalgain;

            /* do long/short dependent scaling operations */
            if (_sideinfo->windowSwitching(_granule_no, channel) &&
                (((_sideinfo->windowType(_granule_no, channel) == 2) &&
                 (!_sideinfo->mixedWindow(_granule_no, channel))) ||
                 ((_sideinfo->windowType(_granule_no, channel) == 2) &&
                 _sideinfo->mixedWindow(_granule_no, channel) &&
                 (subband >= 2)) )){
                    _restored_samples[channel][subband][subsamples] *=
                        pow(2.0, 0.25 * -8.0 *
                            _sideinfo->shortwinGain(_granule_no,
                                       channel, (boundary - cb_begin) / cb_width));

                    _restored_samples[channel][subband][subsamples] *=
                        pow(2.0, scalefactor_scale *
                            _scalefactor->_shortwin_scalefac[_granule_no][channel][(boundary - cb_begin) / cb_width][critical_band]);
            }else{
                _restored_samples[channel][subband][subsamples] *=
                    pow(2.0, scalefactor_scale * (_scalefactor->_longwin_scalefac[_granule_no][channel][critical_band] +
                        _sideinfo->preflag(_granule_no, channel) * _sideinfo->preEmphasis(critical_band)));
            }

            _restored_samples[channel][subband][subsamples] *=
                pow(abs((int)_dehuffman_samples[channel][subband][subsamples]), 4.0/3.0);

            if(_dehuffman_samples[channel][subband][subsamples] < 0){
                _restored_samples[channel][subband][subsamples] = -_restored_samples[channel][subband][subsamples];
            }

#ifdef DEBUG
//    extern std::ofstream test;
//
//    //test << "granuel no: " << _granule_no << std::endl;
//    //test << "channel: " << channel << std::endl;
//    //test << "boundary: " << critical_band << std::endl;
//    test << _restored_samples[channel][subband][subsamples] << '\t';
//    //test << _sideinfo->preflag(_granule_no, channel) << '\t';
//    //test << _sideinfo->preEmphasis(critical_band) << std::endl;
#endif

        }
//#ifdef DEBUG
//    extern std::ofstream test;
//    test << std::endl;
//#endif
    }

#ifdef DEBUG
    extern std::ofstream test;

    //test << std::endl;
#endif

    return true;
}

/* decode joint stereo to MDCT coefficient */
bool MPEGGranule::decodeJointStereo(){
    /* second bit in mode_ext frame header indicates ms_stereo */
    const bool ms_stereo = (_frame_header->modeFlag() == JSTEREO) &&
                           (_frame_header->modeExt() & 0x2);

    /* first bit in mode_ext frame header indicates intensity_stereo */
    const bool i_stereo = (_frame_header->modeFlag() == JSTEREO) &&
                          (_frame_header->modeExt() & 0x1);
//#ifdef DEBUG
//    extern std::ofstream test;
//
//    test << "ms_stereo: " << ms_stereo << '\t' << "i_stereo: " << i_stereo << std::endl;
//#endif

    char is_pos[GRANULE_SAMPLES];
    double is_ratio[GRANULE_SAMPLES];
    double k[CHANNEL][GRANULE_SAMPLES];
    const int channel = 0;
    int i, j, subsamples, subband;
    int lines;
    int scalefactor_band, scalefactor_band_count, max_scalefactor_band;
    const double pi12 = PI/12;

    /* initialization */
    memset((void *)&is_pos[0], 7, GRANULE_SAMPLES * sizeof(char));

    /* intensity stereo processing */
    const int s_channel = (_frame_header->modeFlag() == SINGLECH)?1:2;
    const int s_version = _frame_header->version();
    if((s_channel == 2) && i_stereo){
        if (_sideinfo->windowSwitching(_granule_no, channel) &&
            (_sideinfo->windowType(_granule_no, channel) == 2)){
                if(_sideinfo->mixedWindow(_granule_no, channel)){
                    max_scalefactor_band = 0;
                    for(j = 0; j < SUBBLOCK_COUNTS; j++){
                        scalefactor_band_count = 2;
                        for(scalefactor_band = 12; scalefactor_band >= 3; scalefactor_band--){
                            lines = _frame_header->shortWinScalefactorBandBound(scalefactor_band + 1) -
                                    _frame_header->shortWinScalefactorBandBound(scalefactor_band);
                            i = 3 * _frame_header->shortWinScalefactorBandBound(scalefactor_band) +
                                (j + 1) * lines - 1;
                            while(lines > 0){
                                if(_restored_samples[channel + 1][i / SUBBAND_SAMPLES][i % SUBBAND_SAMPLES] != 0.0){
                                    scalefactor_band_count = scalefactor_band;
                                    scalefactor_band = -10;
                                    lines = -10;
                                }
                                lines--;
                                i--;
                            }
                        }
                        scalefactor_band = scalefactor_band_count + 1;

                        if (scalefactor_band > max_scalefactor_band){
                            max_scalefactor_band = scalefactor_band;
                        }

#ifdef DEBUG
    extern std::ofstream test;

    //test << "max scalefactor band: " << max_scalefactor_band << std::endl;
#endif

                        while (scalefactor_band < 12){
                            subband = _frame_header->shortWinScalefactorBandBound(scalefactor_band + 1) -
                                      _frame_header->shortWinScalefactorBandBound(scalefactor_band);
                            i = 3 * _frame_header->shortWinScalefactorBandBound(scalefactor_band) + j * subband;
                            for(; subband > 0; subband--){
                                is_pos[i] = _scalefactor->_shortwin_scalefac[_granule_no][channel + 1][j][scalefactor_band];
                                if(is_pos[i] != 7){             /* only MPEG1 encoding is processed */
                                    if (s_version == MPEG2){
                                        printf("MPEG2 is not supported.\n");
                                    }else{
                                        is_ratio[i] = tan((float)is_pos[i] * pi12);
                                    }
                                    i++;
                                }
                            }

                            scalefactor_band++;
                        }

                        subband = _frame_header->shortWinScalefactorBandBound(12) -
                                  _frame_header->shortWinScalefactorBandBound(11);
                        scalefactor_band = 3 * _frame_header->shortWinScalefactorBandBound(11) + j * subband;
                        subband = _frame_header->shortWinScalefactorBandBound(13) -
                                  _frame_header->shortWinScalefactorBandBound(12);

                        i = 3 * _frame_header->shortWinScalefactorBandBound(11) + j * subband;
                        for(; subband > 0; subband--){
                            is_pos[i] = is_pos[scalefactor_band];
                            is_ratio[i] = is_ratio[scalefactor_band];
                            k[0][i] = k[0][scalefactor_band];
                            k[1][i] = k[1][scalefactor_band];
                            i++;
                        }
                    }
                    if (max_scalefactor_band <= 3){
                        i = 2;
                        subsamples = 17;
                        subband = -1;
                        while(i >= 0){
                            if(_restored_samples[channel + 1][i][subsamples] != 0.0){
                                subband = i * SUBBAND_SAMPLES + subsamples;
                                i = -1;
                            }else{
                                subsamples--;
                                if(subsamples < 0){
                                    i--;
                                    subsamples = 17;
                                }
                            }
                        }
                        i = 0;

                        while(((int)_frame_header->longWinScalefactorBandBound(i)) <= subband){
                            i++;
                        }
                        scalefactor_band = i;

                        i = _frame_header->longWinScalefactorBandBound(i);
                        for(; scalefactor_band < 8; scalefactor_band++){
                            subband = _frame_header->longWinScalefactorBandBound(scalefactor_band + 1) -
                                      _frame_header->longWinScalefactorBandBound(scalefactor_band);
                            for(; subband > 0; subband--){
                                is_pos[i] = _scalefactor->_longwin_scalefac[_granule_no][channel + 1][scalefactor_band];
                                if (is_pos[i] != 7){
                                    if (s_version == MPEG2){
                                        printf("MPEG2 encoding is not supported.\n");
                                    }else{
                                        is_ratio[i] = tan((float)is_pos[i] * pi12);
                                    }
                                    i++;
                                }
                            }
                        }
                    }
                }else{          /* short blocks */
                    for (j = 0; j < 3; j++){
                        scalefactor_band_count = -1;
                        for(scalefactor_band = 12; scalefactor_band >= 0; scalefactor_band--){
                            lines = _frame_header->shortWinScalefactorBandBound(scalefactor_band + 1) -
                                    _frame_header->shortWinScalefactorBandBound(scalefactor_band);
                            i = 3 * _frame_header->shortWinScalefactorBandBound(scalefactor_band) + (j + 1) * lines - 1;
                            while(lines > 0){
                                if(_restored_samples[channel + 1][i / SUBBAND_SAMPLES][i % SUBBAND_SAMPLES] != 0.0){
                                    scalefactor_band_count = scalefactor_band;
                                    scalefactor_band = -10;
                                    lines = -10;
                                }
                                lines--;
                                i--;
                            }
                        }
                        scalefactor_band = scalefactor_band_count + 1;
                        while(scalefactor_band < 12){
                            subband = _frame_header->shortWinScalefactorBandBound(scalefactor_band + 1) -
                                      _frame_header->shortWinScalefactorBandBound(scalefactor_band);
                            i = 3 * _frame_header->shortWinScalefactorBandBound(scalefactor_band) + j * subband;
                            for(; subband > 0; subband--){
                                is_pos[i] = _scalefactor->_shortwin_scalefac[_granule_no][channel][j][scalefactor_band];
                                if (is_pos[i] != 7){
                                    if (s_version == MPEG2){
                                        printf("MPEG2 encoding is not supported");
                                    }else{
                                        is_ratio[i] = tan((float)is_pos[i] * pi12);
                                    }
                                    i++;
                                }
                            }
                            scalefactor_band++;
                        }

                        subband = _frame_header->shortWinScalefactorBandBound(12) -
                                  _frame_header->shortWinScalefactorBandBound(11);
                        scalefactor_band = 3 * _frame_header->shortWinScalefactorBandBound(11) +
                                           j * subband;
                        subband = _frame_header->shortWinScalefactorBandBound(13) -
                                  _frame_header->shortWinScalefactorBandBound(12);

                        i = 3 * _frame_header->shortWinScalefactorBandBound(11) + j * subband;
                        for(; subband > 0; subband--){
                            is_pos[i] = is_pos[scalefactor_band];
                            is_ratio[i] = is_ratio[scalefactor_band];
                            k[0][i] = k[0][scalefactor_band];
                            k[1][i] = k[1][scalefactor_band];
                            i++;
                        }
                    }
                }
        }else{              /* long blocks */
            i = 31;
            subsamples = 17;
            subband = 0;
            while(i >= 0){
                if (_restored_samples[channel + 1][i][subsamples] != 0.0){
                    subband = i * 18 + subsamples;
                    i = -1;
                }else{
                    subsamples--;
                    if(subsamples < 0){
                        i--;
                        subsamples = 17;
                    }
                }
            }
            i = 0;
            while((int)_frame_header->longWinScalefactorBandBound(i) <= subband){
                i++;
            }
            scalefactor_band = i;
            i = _frame_header->longWinScalefactorBandBound(i);
            for (; scalefactor_band < 21; scalefactor_band++){
                subband = _frame_header->longWinScalefactorBandBound(scalefactor_band + 1) -
                          _frame_header->longWinScalefactorBandBound(scalefactor_band);
                for(; subband > 0; subband--){
                    is_pos[i] = _scalefactor->_longwin_scalefac[_granule_no][channel + 1][scalefactor_band];
                    if (is_pos[i] != 7){
                        if (s_version == MPEG2){
                            printf("MPEG2 encoding is not supported.\n");
                        }else{
                            is_ratio[i] = tan((float)is_pos[i] * pi12);
                        }
                        i++;
                    }
                }
            }
            scalefactor_band = _frame_header->longWinScalefactorBandBound(20);
            for(subband = 576 - _frame_header->longWinScalefactorBandBound(21); subband > 0; subband--){
                is_pos[i] = is_pos[scalefactor_band];
                is_ratio[i] = is_ratio[scalefactor_band];
                k[0][i] = k[0][scalefactor_band];
                k[1][i] = k[1][scalefactor_band];
                i++;
            }
        }
    }

    /* write stereo values to buffer */
    if(s_channel == 2){
        for(subband = 0; subband < SUBBAND_COUNTS; subband++){
            for(subsamples = 0; subsamples < SUBBAND_SAMPLES; subsamples++){
                i = subband * SUBBAND_SAMPLES + subsamples;
#ifdef DEBUG
    extern std::ofstream test;

    //test << "i: " << i << '\t';
    //test << "is_pos: " << (int)is_pos[i] << '\t';
    //test << "is_ratio: " << is_ratio[i] << '\t';
#endif
                if(is_pos[i] == 7){
                    if(ms_stereo){
                        _mdct_coefficients[0][subband][subsamples] = (_restored_samples[0][subband][subsamples] +
                            _restored_samples[1][subband][subsamples]) / SQRT_2;
                        _mdct_coefficients[1][subband][subsamples] = (_restored_samples[0][subband][subsamples] -
                            _restored_samples[1][subband][subsamples]) / SQRT_2;
                    }else{
                        _mdct_coefficients[0][subband][subsamples] = _restored_samples[0][subband][subsamples];
                        _mdct_coefficients[1][subband][subsamples] = _restored_samples[1][subband][subsamples];
                    }
                }else if(i_stereo){
                    if (s_version == MPEG2){
                        printf("MPEG2 encoding is not supported.\n");
                    }else{
                        _mdct_coefficients[0][subband][subsamples] = _restored_samples[0][subband][subsamples] * (is_ratio[i] / (1 + is_ratio[i]));
                        _mdct_coefficients[1][subband][subsamples] = _restored_samples[0][subband][subsamples] * (1 / (1 + is_ratio[i]));
                    }
                }else{
                    printf("Error in joint stereo processing.\n");
                }
#ifdef DEBUG
    test << "left: " << _mdct_coefficients[0][subband][subsamples] << '\t';
    test << "right: " << _mdct_coefficients[1][subband][subsamples] << '\t';
#endif
            }
#ifdef DEBUG
    extern std::ofstream test;

    test << std::endl;
#endif
        }
    }else{                  /* mono */
#ifdef DEBUG
    assert(false);
#endif
        memcpy((void *)&_mdct_coefficients[0][0][0],
               (void *)&_restored_samples[0][0][0], GRANULE_SAMPLES * sizeof(double));
    }

    return true;
}

//void MPEGGranule::reorder(int channel){
//    int scalefactor_band, start, lines;
//    int window, freq, src_line, des_line;
//
//    if (_sideinfo->windowSwitching(_granule_no, channel) &&
//        (_sideinfo->windowType(_granule_no, channel) == 2)){
//            if (_sideinfo->mixedWindow(_granule_no, channel)){
//                /* no reorder for low 2 subbands */
//                memcpy((void*)&_reordered_samples[channel],
//                       (void *)&_mdct_coefficients[channel],
//                       sizeof(double) * 2 * SUBBAND_SAMPLES);
//
//                /* reordering for rest switched short */
//                start = _frame_header->shortWinScalefactorBandBound(3);
//                lines = _frame_header->shortWinScalefactorBandBound(4) - start;
//                scalefactor_band = 3;
//            }else{                  /* pure short window, start from 0 */
//                start = 0;
//                lines = _frame_header->shortWinScalefactorBandBound(1);
//                scalefactor_band = 0;
//            }
//
//            for(; scalefactor_band < 13; scalefactor_band++){
//                for(window = 0; window < 3; window++){
//                    for(freq = 0; freq < scalefactor_band; freq++){
//                        src_line = start * 3 + window * lines + freq;
//                        des_line = start * 3 + window + freq * 3;
//                        _reordered_samples[channel][des_line/SUBBAND_SAMPLES][des_line%SUBBAND_SAMPLES] =
//                            _mdct_coefficients[channel][src_line/SUBBAND_SAMPLES][src_line%SUBBAND_SAMPLES];
//                    }
//                }
//
//                start = _frame_header->shortWinScalefactorBandBound(scalefactor_band);
//                lines = _frame_header->shortWinScalefactorBandBound(scalefactor_band + 1) - start;
//            }
//    }else{                          /* long blocks */
//        /* copy array _mdct_coefficients to result buffer */
//        memcpy((void *)&_reordered_samples[channel], (void *)&_mdct_coefficients[channel],
//            sizeof(double) * GRANULE_SAMPLES);
//    }
//
//    return;
//}

///* if the effect is not so good, reimplement this section */
///* align block, i.e. evaluate means of three consecutive samples */
//void MPEGGranule::align(int channel){
//    int des_idx = 0;
//    int src_idx = 0;
//    double *data = (double *)_mdct_coefficients;
//
//    while(src_idx < GRANULE_SAMPLES){
//        _aligned_samples[channel][des_idx] += abs(data[src_idx++]);
//        _aligned_samples[channel][des_idx] += abs(data[src_idx++]);
//        _aligned_samples[channel][des_idx] += abs(data[src_idx++]);
//
//        _aligned_samples[channel][des_idx++] /= 3;
//    }
//}